import numpy as np

from .model import Model
from ..utils import TransformationUtils


class Vehicle(Model):
    def __init__(self) -> None:
        super().__init__()
        self._nq = 3
        self._x = np.zeros(self._nq)
        self._min_theta = 1e-6

    @property
    def nq(self):
        return self._nq

    @property
    def x(self):
        return self._x.copy()

    @x.setter
    def x(self, x_: np.ndarray):
        self._x[:] = x_

    def F(self, x, v):

        # f = np.array([
        #     x[0] + v[0] * np.cos(x[2] + v[1]),
        #     x[1] + v[0] * np.sin(x[2] + v[1]),
        #     x[2] + v[1]
        # ])

        if np.abs(v[1]) < self._min_theta:
            f = np.array([
                x[0] + v[0] * np.cos(x[2]),
                x[1] + v[0] * np.sin(x[2]),
                x[2] + v[1]
            ])
        else:
            f = np.array([
                x[0] + (np.sin(x[2] + v[1]) - np.sin(x[2])) * v[0] / v[1],
                x[1] + (np.cos(x[2]) - np.cos(x[2] + v[1])) * v[0] / v[1],
                x[2] + v[1]
            ])

        return f

    def Fx(self, x, v):
        fx = np.eye(3)

        # fx[0, 2] = -v[0] * np.sin(x[2] + v[1])
        # fx[1, 2] = v[0] * np.cos(x[2] + v[1])

        if np.abs(v[1]) < self._min_theta:
            fx[0, 2] = -v[0] * np.sin(x[2])
            fx[1, 2] = v[0] * np.cos(x[2])
        else:
            fx[0, 2] = (np.cos(x[2] + v[1]) - np.cos(x[2])) * v[0] / v[1]
            fx[1, 2] = (np.sin(x[2] + v[1]) - np.sin(x[2])) * v[0] / v[1]

        return fx

    def Fv(self, x, v):
        # fv = np.array([
        #     [np.cos(x[2] + v[1]), -v[0] * np.sin(x[2] + v[1])],
        #     [np.sin(x[2] + v[1]), v[0] * np.cos(x[2] + v[1])],
        #     [0, 1]
        # ])

        if np.abs(v[1]) < self._min_theta:
            fv = np.array([
                [np.cos(x[2]), 0],
                [np.sin(x[2]), 0],
                [0, 1]
            ])
        else:
            s_x2 = np.sin(x[2])
            c_x2 = np.cos(x[2])
            s_x2_v1 = np.sin(x[2] + v[1])
            c_x2_v1 = np.cos(x[2] + v[1])

            fv = np.array([
                [(s_x2_v1 - s_x2) / v[1], ((s_x2 - s_x2_v1) / v[1] + c_x2_v1) * v[0] / v[1]],
                [(c_x2 - c_x2_v1) / v[1], ((c_x2_v1 - c_x2) / v[1] + s_x2_v1) * v[0] / v[1]],
                [0, 1]
            ])

        return fv

    def handle_state(self, x):
        x[2] = TransformationUtils.wrap(x[2])[0]
        return x
